package engine.util.math;

public class Quaternion {

	private float x;
	private float y;
	private float z;
	private float w;
	
	public Quaternion(float x, float y, float z, float w) {
		this.x = x;
		this.y = y;
		this.z = z;
		this.w = w;
	}
	
	public static Quaternion conjugate(Quaternion conj) {
		return new Quaternion(-conj.getX(), -conj.getY(), -conj.getZ(), conj.getW());
	}
	
	public static Quaternion invert(Quaternion inv) {
		float mag = (float)Math.pow(magnitude(inv), 2);
		return new Quaternion(-inv.getX() / mag, -inv.getY() / mag, -inv.getZ() / mag, inv.getW() / mag);
	}
	
	public static float magnitude(Quaternion mag) {
		return (float)Math.sqrt(mag.getX() * mag.getX() + mag.getY() * mag.getY() + mag.getZ() * mag.getZ() + mag.getW() * mag.getW());
	}
	
	public static Quaternion normalize(Quaternion norm) {
		float mag = magnitude(norm);
		
		return new Quaternion(norm.getX() / mag, norm.getY() / mag, norm.getZ() / mag, norm.getW() / mag);
	}
	
	public static Quaternion mult(Quaternion left, Quaternion right) {
		return new Quaternion(
				left.getW() * right.getX() + left.getX() * right.getW() + left.getY() * right.getZ() - left.getZ() * right.getY(),
				left.getW() * right.getY() + left.getY() * right.getW() + left.getZ() * right.getX() - left.getX() * right.getZ(),
				left.getW() * right.getZ() + left.getZ() * right.getW() + left.getX() * right.getY() - left.getY() * right.getX(),
				left.getW() * right.getW() - left.getX() * right.getX() - left.getY() * right.getY() - left.getZ() * right.getZ());
	}
	
	public static Quaternion convRotAxisToQuat(float angle, float x, float y, float z) {
		float sin = (float)Math.sin(angle / 2);
		
		return normalize(new Quaternion(x * sin, y * sin, z * sin, (float)Math.cos(angle / 2)));
	}
	
	public static Quaternion sub(Quaternion left, Quaternion right) {
		return normalize(new Quaternion(left.getX() - right.getX(), left.getY() - right.getY(), left.getZ() - right.getZ(), left.getW() - right.getW()));
	}
	
	public static Quaternion add(Quaternion left, Quaternion right) {
		return normalize(new Quaternion(left.getX() + right.getX(), left.getY() + right.getY(), left.getZ() + right.getZ(), left.getW() + right.getW()));
	}
	
	public static Quaternion divide(Quaternion up, Quaternion down) {
		float mag = down.getX() * down.getX() + down.getY() * down.getY() + down.getZ() * down.getZ() + down.getW() * down.getW();
		return new Quaternion(
				(down.getW() * up.getX() - down.getX() * up.getW() - down.getY() * up.getZ() + down.getZ() * up.getY()) / mag, 
				(down.getW() * up.getY() + down.getX() * up.getZ() - down.getY() * up.getW() - down.getZ() * up.getX()) / mag, 
				(down.getW() * up.getZ() - down.getX() * up.getY() + down.getY() * up.getX() - down.getZ() * up.getW()) / mag, 
				(down.getW() * up.getW() + down.getX() * up.getX() + down.getY() * up.getY() + down.getZ() * up.getZ()) / mag);
	}
	
	public float[] getForwardVector() {
		return new float[]{2 * (x * z + w * y), 2 * (y * x - w * x), 1 - 2 * (x * x + y * y)};
	}
	 
	public float[] getUpVector() {
		return new float[]{2 * (x * y - w * z), 1 - 2 * (x * x + z * z), 2 * (y * z + w * x)};
	}
	 
	public float[] getRightVector() {
		return new float[]{1 - 2 * (y * y + z * z), 2 * (x * y + w * z), 2 * (x * z - w * y)};
	}
	
	public float getX() {
		return x;
	}
	
	public float getY() {
		return y;
	}
	
	public float getZ() {
		return z;
	}
	
	public float getW() {
		return w;
	}
	
	public void setX(float x) {
		this.x = x;
	}

	public void setY(float y) {
		this.y = y;
	}
	
	public void setZ(float z) {
		this.z = z;
	}
	
	public void setW(float w) {
		this.w = w;
	}
}
